#include "pnp.h"

// Camera intrinsics, TUM Freiburg2
//     (fx  0 cx)   (520.9     0 325.1)
// K = ( 0 fy cy) = (    0 521.0 249.7)
//     ( 0  0  1)   (    0     0     1)
Point2d pixel2cam(const Point2d &p, const Mat &K)
{
  return Point2d(
      (p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),
      (p.y - K.at<double>(1, 2)) / K.at<double>(1, 1));
}
